An Occupancy Mapping Method Based on K-Nearest Neighbours

OctoMap is an efficient probabilistic mapping framework to build occupancy maps from point clouds, representing 3D environments with cubic nodes in the octree. However, the map update policy in OctoMap has limitations. All the nodes containing points will be assigned with the same probability regardless of the points being noise, and the probability of one such node can only be increased with a single measurement. In addition, potentially occupied nodes with points inside but traversed by rays cast from the sensor to endpoints will be marked as free. To overcome these limitations in OctoMap, the current work presents a mapping method using the context of neighbouring points to update nodes containing points, with occupancy information of a point represented by the average distance from a point to its k-Nearest Neighbours. A relationship between the distance and the change in probability is defined with the Cumulative Density Function of average distances, potentially decreasing the probability of a node despite points being present inside. Experiments are conducted on 20 data sets to compare the proposed method with OctoMap. Results show that our method can achieve up to 10% improvement over the optimal performance of OctoMap.


Introduction
As an environment perception approach for autonomous mobile robots, occupancy mapping is widely used in path planning [1,2], navigation [3,4] and autonomous driving [5,6]. In most applications, occupancy maps are generated from point clouds. Sensors such as LIDAR [7,8] can produce high-quality point clouds to represent the 3D world. However, such sensors are normally expensive, which has restricted their application. Currently, point clouds are becoming increasingly popular in the research community as cheaper solutions become available. An RGB-D camera [9,10] can simultaneously produce both colour and depth images, the latter of which can be used for point cloud reconstruction. Similarly, point clouds can be reconstructed from a stereo camera [11,12] with the disparity map derived by left and right images.
To build occupancy maps from point clouds, OctoMap is a popular mapping algorithm. OctoMap is octree-structure-based and uses probabilistic occupancy estimation to update maps [13]. An octree is a hierarchical data structure in which each node is recursively divided into eight children [14,15]. In OctoMap, the volumes of a 3D space are represented by the cubic nodes in the octree and the probability of each node is updated with the measured endpoints in point clouds. It is assumed that the endpoints correspond to obstacle surfaces and there are no objects between sensor origin and endpoints. A raycasting operation from the senor to endpoints will be performed to determine which nodes should be updated. A node will be updated accordingly with the probabilities in the inverse sensor model if the node is traversed by the beam cast from the sensor or the beam is reflected in the node.
However, the map update policy in OctoMap has limitations when dealing with nodes containing points. Normally, point clouds are either generated by sensors directly or by implementing algorithms on the original data from sensors. Due to sensor noise and the limitations of point cloud generation algorithms, a point cloud normally contains both points on the external surfaces of objects and noise points. One one hand, in OctoMap, the nodes containing points are updated with the same probability regardless of points being noise. On the other hand, with a single frame of point cloud, the probability of a node with points inside can be increased but is never allowed to be decreased. In addition, potentially occupied nodes containing points but traversed by rays will be marked as free. The limitations of the update policy in OctoMap have a negative impact on the mapping performance. Since OctoMap classifies the nodes in a map into occupied and free ones, it can be treated as a binary classifier. The classification performance can be described by a confusion matrix [16] in which elements are categorised into true positives (TP), false positives (FP), true negatives (TN) and false negatives (FN). The occupied nodes related to noise points will be classified as FPs and nodes with points inside but traversed by rays are likely to introduce FNs.
Considering occupancy mapping using point clouds has a wide range of applications in multiple fields, and the map update policy can be improved to enhance the mapping performance. Point cloud filters such as those in the Point Cloud Library (PCL) [7] can be used for denoising. Instead of building a map with the original point clouds, the quality of the map can be improved using the filtered point clouds. However, the details in environmental features may not be preserved when the outliers or clusters in point clouds are removed by filters. To refine occupancy maps, Recurrent-OctoMap is proposed in [17], with each cell in OctoMap modelled as a Recurrent Neural Network (RNN). The learning approach is used to fuse semantic features. In [18], learning-aided 3D occupancy mapping is introduced to deal with sparse and noisy range sensor data. The occupancy states of the unobserved regions can be predicted to build dense occupancy maps. Previous studies reported in the literature also focused on the improvement of the efficiency of occupancy mapping. In [19], a computationally efficient probabilistic map update policy utilising the sparse nature of the environment is proposed. The efficiency of the OctoMap framework can also be improved by the Fast Line Rasterisation Algorithm [20].
The purpose of this work is to overcome the limitations in OctoMap by representing occupancy information with the average distance from a point to its k-Nearest Neighbours (k-NN). The main contributions of this paper are: • a k-NN method for occupancy mapping using the context of neighbouring points to update nodes containing points; • definition of the relationship between the average distance and the change in occupancy probability, potentially decreasing the probability of a node despite the points being present in the node; • the proposed k-NN method is verified by the point clouds derived by the StereoSGBM algorithm [21] implemented on the images produced from a stereo camera, and can be potentially extended to other point-cloud-based mapping systems.
Both OctoMap and the k-NN-based mapping approach are governed by several parameters, the choice of which affects the quality of the final map. To compare the k-NN method with OctoMap, it is reasonable to compare their optimal mapping performance, which can normally be achieved by tuning parameters. A two-step methodology is introduced in [22] to identify optimal parameter sets to improve occupancy mapping performance by first reducing parameters of lower impact using Neighbourhood Component Analysis (NCA) [23] and then optimising the residual most significant ones with grid search. In [22], OctoMap is implemented on 20 data sets collected in two environments to show the effectiveness of the parameter reduction and optimisation method. In this work, the data sets introduced in [22] are used to demonstrate the benefits of the novel k-NN method over OctoMap by comparing their optimal performance derived by the two-step principled methodology as per [22]. This paper is organised as follows. We first introduce the background of OctoMap in Section 2. Then, the k-NN-based inverse sensor model is presented in Section 3, using the relationship between the average distance from a point to its k-NN and occupancy probability. The relationship is defined with the Cumulative Density Function (CDF) of average distances and the choice of the distribution is explained. The map update procedure using the proposed model is given as well. In addition, considerations of parameter space are specified, followed by the introduction of parameter reduction based on NCA and optimisation using grid search. In Section 4, the details of the experiments are given. The results of average distances fitted by different distributions, k-NN parameter reduction and the comparison of the k-NN method and OctoMap are also presented. Finally, the key findings of the CDF of the average distance nonsensitive to distributions, lower impact of parameter compared to other k-NN parameters and the improvement by the k-NN method up to 10% over the optimal performance of OctoMap are discussed.

Background
We compare the k-NN method with OctoMap, the details of which have been explained in [13]. A brief summary of the update policy of OctoMap is introduced here.
In OctoMap, sensor readings are integrated with the occupancy grid mapping method introduced in [24]. Let 1: denote sensor measurements up to time . The probability of a node can be written as: where ( | ) is the probability given measurement , and ( ) denotes the prior probability and is commonly set to 0.5. The log-odds notation can be used to simplify (1) as: where ( | ) is the inverse sensor model. As introduced in Section 1, the nodes that need to be updated are determined by a ray-casting operation from sensor origin to endpoints.
The probabilities corresponding to nodes containing points and traversed by the beam are given in the inverse sensor model: where and are the respective log-odds values to update occupied and free cells. The log-odds value of the probability is limited by the clamping update policy proposed in [25]: where max and min are the upper and lower bounds on the probability in log-odds notation, respectively.

Method
In this work, the context of neighbouring points is used to update occupancy maps. We first define the relationship between the average distance from a point to its k-NN and the occupancy probability. An inverse sensor model is proposed based on the relationship. Using the proposed model, the mapping algorithm is governed by several parameters. These parameters are reduced by NCA and optimised with grid parameter space to achieve the best mapping performance.

K-NN-Based Inverse Sensor Model
In a point cloud, a point is likely to be noise if it is isolated from the points nearby. Based on this assumption, the average distance from a point to its k-NN can be used to represent the occupancy information of this point. A point should be assigned with a higher probability if it has a smaller average distance, and vice versa. We have defined a relationship between the average distance and the change in probability in [26]. In this paper, we make minor changes and redefine the relationship. Let ( ) denote the distribution of the average distance. Then, the probability representing the occupancy information of a point can be denoted as: where is the average distance from a point to its k-NN, and are the upper and lower bounds on the probability, and ( ) is the CDF of the average distance and ( ) = ∫ −∞ ( ) .
Based on the relationship, we define the inverse sensor model as: where is the maximum range for how long individual beams are inserted, and and are the respective log-odds values assigned to the nodes traversed by rays cast from the sensor to the points whose distances to the sensor are within and outside the range . If a node satisfies both the requirements of and , it will be updated with only. Then, we can use (3) and (5) to update an occupancy map.
With the above inverse sensor model and the OctoMap parameters introduced in [22], the parameters of k-NN mapping are as follows.
• max is the upper clamping threshold, which is the upper bound on the probability. • is the threshold. A node will be marked as occupied when the threshold is reached. • is the probability of a "miss". A node will be updated with if it is traversed by rays and corresponding endpoints are within range . • is the probability of a "miss". A node will be updated with if it is traversed by rays and corresponding endpoints are outside range . • min is the lower clamping threshold, which is the lower bound on the probability. • is the upper bound on the probability derived by the average distance from a point to its k-NN.
• is the lower bound on the probability derived by the average distance from a point to its k-NN.
• is the number of nearest neighbouring points.

Distribution of Average Distances
The average distance of a point is computed by searching its k-NN in the corresponding point cloud among points whose distances to the sensor are within range . We use different distributions, i.e., Generalised Extreme Value (GEV) distribution, log-logistic distribution, Rayleigh distribution, Kernel Density Estimation (KDE) and normal distribution, to fit the average distances of all the points within range in a point cloud set generated from one data set. The results are presented in Section 4.4. The CDF of the average distance is nonsensitive to the types of distributions. Although KDE can fit the average distance better than other distributions, it would be difficult to change the k-NN model if KDE is applied due to its non-parametric property. Since there is no obvious change in CDF when the distribution is different, we assume that the average distance is subject to a normal distribution: where is the mean and is the standard deviation.
Using the model in Section 3.1 to update occupancy maps, the mean and the standard deviation of the average distances are required. To avoid brute force calculation and improve accuracy, the method for calculating corrected sums of squares and products noted in [27] is implemented, which can reduce rounding errors in computer implementation. As a result, a series of values of mean and standard deviation will be generated as the number of points grows. Let denote the number points. For = 1, 2, . . . , , the following process will be performed: where is the average distance from the th point to its k-NN in the corresponding point cloud, −1 and are the mean values for − 1 and points, −1 and are the sums of the squares of the deviations for − 1 and points, and 0 = 0 and 0 = 0. Then, the mean and the standard deviation of the normal distribution can be derived by: (10)

Map Update
Algorithm 1 shows the mapping process with the k-NN-based inverse sensor model in Section 3.1. Here, represents the point cloud at time . The corresponding position of the vision sensor is denoted as . is the occupancy map. Lines 3 through 10 update the nodes containing points. Lines 11 to 20 update the nodes traversed by rays cast from the sensor to endpoints. The traversed nodes are updated with log-odds values and , corresponding to the probabilities of and .

Parameter Space Considerations
In this work, we use grid parameter space for analysis, which has been introduced in [22]. Each parameter is generated by the corresponding maximum, minimum and step. A set, as introduced in [22], can be used to describe the possible values of any parameter: where max and min are the upper and lower bounds on the parameter, and is the step. The algorithm-required relations with other parameters should also be considered when generating parameters.
≥ since corresponds to points further from the sensor than . A reasonable set of the parameters of the k-NN method should satisfy: Algorithm 1: Map Update Input: , , , , , Output: Two functions are defined in [22] to describe the combinations of parameters: and ℎ( , , ) = min where and are the possible values of two parameters, and are corresponding steps, and is an input combination number.
Considering the relations in (12), the number of combinations of the parameters in the k-NN model is: where the possible values of correspond to the max value, min( max ) + ( − 1) ( max ), and min value, min( min ) + ( − 1) ( min ), and ( ) = .
Our investigation of parameters is presented based on different data sets, an overview of which is given in Section 4.1. Let denote the number of data sets. We first generate a random permutation of the indices of all the possible combinations of k-NN parameters and then divide the combinations into groups according to their indices. The number of combinations for each data set is: = .

Parameter Reduction and Optimisation
We use the method in [22] for parameter reduction and optimisation, first identifying the most significant parameters and then optimising them with grid search.
Both point cloud parameters and mapping parameters will affect the mapping performance. However, given that mapping parameters have a higher impact on the mapping performance than point cloud parameters [22], in this work, we only focus on the reduction and optimisation of k-NN mapping parameters. We use 20 data sets introduced in [22] for experiments. Since the data sets in this work are collected in outdoor environments, most elements in an occupancy map belong to TNs. As introduced in [22], we use the Receiver Operating Characteristic (ROC) variant true positive rate (TPR)-false positive rate (FDR) as a performance metric to deal with such unbalanced data sets.
For parameter reduction, the combination of point cloud parameters remains the same on different data sets. We choose point cloud sets with FDR derived by the non-parametric naive mapping approach proposed in [22] based on their ranks. The choice of the point cloud set or the combination of point cloud parameters for k-NN parameter reduction will be given in Section 4.3. For each combination of k-NN parameters, an occupancy map can be generated using Algorithm 1. By comparing the map with ground truth, the nodes in the map can be classified into four categories, i.e., TPs, FPs, TNs and FNs. Metrics TPR and FDR are computed from the number of nodes in each category. NCA feature selection [23] will be applied to analyse parameter weights under the performance metrics TPR and FDR, which has been introduced in [22]. For ease of comparison, we normalise the weights derived by different data sets as per [23].
Based on the above parameter reduction and the optimisation results of OctoMap parameters in [22], parameters to be optimised can be determined and will be introduced in Section 4.3. Parameters can be optimised by grid search in the parameter space defined in Section 3.4. The area under the curve (AUC) of TPR-FDR variant is used as the performance metric for optimisation. Three point cloud sets in each data set will be selected using the naive approach in [22] to compare the best AUC derived by the k-NN method and that derived by OctoMap.

Run Time
For OctoMap, the time elapsed is proportional to the number of points processed, so the run time can be denoted as: where and are coefficients, and is the number of points in the point cloud set used for generating an occupancy map. The run time of the k-NN-based mapping approach is proportional to parameter and the number of processed points . The run time can be derived by: where , and are coefficients.

Overview of Data Sets
We have introduced the data sets collected with a controlled procedure in [22]. Boxes with either a plain cardboard texture or Voronoi diagrams [28] are the targets to be explored. With a pair of boxes, five layouts can be created with the free tetrominoes in the Teris game [29,30], i.e., I, O, T, L and S. Data sets are collected in two environments, in front of buildings and in a parking lot, with a ZED stereo camera (Stereo Labs, USA) moving along the circle on the ground and orbiting the targets twice. The resolution of a single image is HD (1280 × 720 pixels). The number of data sets is 20 considering textures, layouts and environments. Figure 1a shows one example of 20 data sets, O tetromino layout boxes with the Voronoi pattern in the parking lot. The corresponding camera trajectory for data collection is presented in Figure 1b.  Figure 2 shows the experimental method for comparing two mapping methods. As explained in [22], the StereoSGBM algorithm [21] in OpenCV is implemented on the images of the keyframes derived by ORB-SLAM [31] to generate disparity maps, from which point clouds can be reconstructed with a stereo camera model. These point clouds are then downsampled with the Voxel filter in the PCL [7]. The resolution of the Voxel filter is set to 0.1 m. With time stamps, each point cloud can match with the keyframe pose generated by ORB-SLAM. The resolution of occupancy maps and maximum range for how long individual beams are inserted are set to 0.1 m and 4 m, which has been discussed in [22].

Map Generation and Node Classification
For k-NN parameter reduction, the combination of point cloud parameters remains the same on different data sets. Point cloud sets are chosen with FDR derived by the nonparametric mapping approach proposed in [22] based on their ranks. The choice of the point cloud set or the combination of point cloud parameters for k-NN parameter reduction will be given in Section 4.3. For each combination of k-NN parameters, an occupancy map can be generated from Algorithm 1. By comparing the map with ground truth, the nodes in the map can be classified into four categories using the method in [22]. Metrics TPR and FDR are computed from the number of nodes in each category. NCA feature selection proposed in [23] will be applied to analyse parameter weights, which has been introduced in [22]. For ease of comparison, the weights derived by different data sets are normalised as per [23].
Parameters can be optimised by grid search in the parameter space defined by the method in Section 4.3. Based on the above k-NN parameter reduction results and the optimisation results of OctoMap parameters in [22], parameters to be optimised can be determined and will be introduced in Section 4.3. The AUC of TPR-FDR variant is used as the performance metric for optimisation. Three point cloud sets in each data set will be selected using the non-parametric approach to compare the k-NN method with OctoMap.
In the end, the time estimation models introduced in Section 3.6 will be verified.

Parameter Space for Analysis
In [22], there are 1600 combinations of point cloud parameters, which means that 1600 point cloud sets can be generated for each data set. As specified in Section 3.5, we ranked the point cloud sets by FDR derived by the non-parametric mapping approach as per [22]. (1) For the reduction of k-NN parameters, we can fix the combination of point cloud parameters since they are less important than mapping parameters in performance [22]. We choose the combination of parameters corresponding to the 800th ranked point cloud set of the data set collected with I layout Voronoi boxes in front of buildings. (2) For the optimisation of the mapping parameters, we choose the 1st, 800th and 1600th (lower number indicates better quality, i.e., cleaner point clouds) ranked point cloud sets from each data set to compare the optimal performance of OctoMap and the k-NN method.
The configuration of mapping parameters is shown in Table 1. max , , and min are shared by the two mapping approaches. The choice of the step of the OctoMap parameters has been discussed in [22]; 0.12 is a reasonable step and will not affect the results. Smaller steps have been tested in [22] but no obvious difference has been observed in the results. The step is small enough for the grid search to give valid results. To investigate k-NN parameter weights, max , min and are varied with corresponding steps, and changes with max and min . With (15), the total number of combinations is 112,500. To optimise the k-NN parameters, based on the results in Section 4.4, is constant since it has a lower impact on the performance metrics. max and min are set to 0.98 and 0.02 since these values show the highest frequencies in the optimal values in [22]. Moreover, max and min are the upper and lower bounds on the probability but not the parameters for inverse senor models, and they are shared by both the k-NN method and OctoMap. Therefore, locking these two parameters does not benefit any approach but can decrease the number of combinations of parameters to reduce the computational time. Besides max and min , the setup of other OctoMap parameters is dependent on the configuration in [22]. To compare the k-NN method and OctoMap, the combinations of parameters for the two mapping algorithms are 4050 and 180, respectively.

Results
We first show the results of the average distance fitted by different distributions, i.e., GEV distribution, log-logistic distribution, Rayleigh distribution, KDE and normal distribution. An example is given by the average distance derived by the 800th point cloud set of I layout Voronoi boxes in front of buildings. The corresponding CDF is presented in Figure 3. Results show that the CDF of the average distance is nonsensitive to the types of distribution. Results from other point cloud sets also show a similar conclusion.  Then, the results of the weights of k-NN parameters under performance metrics TPR and FDR are presented. As specified in Section 4.3, point cloud parameters are consistent in the 20 data sets. With the configuration of mapping parameters in Table 1, the weights of k-NN parameters are computed by implementing NCA feature selection on TPR and FDR derived by the node classification results. With (15) and (16), the number of parameter combinations for each data set is 5625. The normalised weight of each k-NN parameter is shown in Figure 4. Overall, parameter is less important than the other parameters. Its weight is under 0.1 in both performance metrics. Therefore, parameter can be excluded from the optimisation for the best mapping performance. We also compare the performance of the k-NN method and OctoMap. The 1st, 800th and 1600th ranked point cloud sets generated from each data set are selected to optimise the performance of each mapping algorithm. The optimal performance of the two mapping algorithms is then compared. All the parameters except max , min , and will be optimised by searching the optimal AUC of TPR-FDR variant using the grid parameter space defined with Table 1.
is varied to generate points on the TPR-FDR curve. For any combination of parameters in each mapping algorithm, nine points will be produced. A point will be discarded if its metric is not a number. Figure 5 shows the improvement achieved by the k-NN method over the optimal AUC derived by OctoMap on 20 data sets. The improvement increases as the optimal AUC of OctoMap decreases, but can be negative when the AUC of OctoMap is relatively large. With the parameter configuration in Table 1, we can achieve an improvement up to 10%. Overall, the mapping performance of Voronoi boxes is better than that of plain boxes.   Finally, the time model in Section 3.6 is verified. Here, the point cloud parameters are the same as those used for k-NN parameter reduction, introduced in Section 4.3. Figure 7a presents the linear fit for the run time of OctoMap. Corresponding coefficients in (17) are = 2.3371 × 10 −5 and = 6.9527 × 10 −1 . Then, 5% of the parameter reduction results with the aforesaid point cloud parameters in [22] are randomly selected to plot Figure 7a. The result shows that the run time is proportional to the number of points processed by the OctoMap algorithm. Figure 7b verifies the time estimation model (18). In each data set, 5% of the k-NN parameter reduction results are randomly selected to estimate the time model. The coefficients in the model are = 7.3592 × 10 −7 , = 2.7681 × 10 −5 and = 1.5148. The result shows that run time increases with parameter and the number of points processed.

Discussion
The CDF of the average distance is nonsensitive to different distributions. Given that the CDF is implemented to define the relationship between the average distance and the change in the occupancy probability, we use a parametric distribution for ease of adjusting the k-NN model. This can be potentially useful since the distribution of the average distance might be different when the environment is changed. With a parametric distribution, the k-NN model can be changed to adapt to different environments.
Among all the k-NN parameters, parameter has a lower impact on the performance metrics and thus can be fixed. In addition, based on the optimisation of Oc-toMap parameters in [22], max and min can be set as constants to further reduce the computational complexity.
The optimal AUC of k-NN shows an improvement of up to 10% over that of OctoMap. The improvement achieved by the k-NN method increases as the optimal AUC of OctoMap decreases but can be negative when the optimal AUC of OctoMap is relatively large. Overall, the mapping performance is better in the environment with buildings since there are more image features on the objects nearby and the quality of point clouds is better. In each environment, the mapping performance is normally better when targets are covered with Voronoi diagrams due to the extra features introduced by the diagrams. There is no obvious trend among different layouts.
In Figure 5, the improvement by the k-NN method against OctoMap can be observed on most data sets in terms of AUC. However, when the k-NN method achieves optimal improvement, the TPR-FDR curves derived by the two mapping algorithms may intersect, i.e., points on the curve derived by the k-NN method are always better than those on the curve derived by OctoMap when FDR is smaller than that of the intersection point, while they are worse when FDR is larger than that of the intersection point. However, normally, a combination of k-NN parameters can be found whose improvement against the optimal AUC of OctoMap is less significant than that achieved in Figure 5 such that, for each point on the TPR-FDR curve of OctoMap, a point of better performance can be found on the curve derived by the k-NN method.
For both occupancy mapping methods, run time is proportional to the number of points processed. OctoMap is faster than the k-NN method when processing the same point cloud set. The run time of the k-NN method increases with parameter . Since the change in has little impact on the performance metrics, a smaller can be chosen to reduce the computational time.

Conclusions
In this paper, we present an inverse sensor model for occupancy mapping using the context of neighbouring points. The occupancy information of a point is represented with the average distance to its k-NN. The relationship between the average distance and the occupancy probability is defined with the corresponding CDF. By implementing NCA, the parameter that has a lower impact on the mapping performance can be reduced. In addition, by considering the optimal values of OctoMap parameters, the number of parameters to be investigated can be further reduced. Through searching the grid parameter space, the residual most important parameters can be optimised to achieve the optimal performance. We implement the k-NN method on point clouds derived by different data sets. Results show that the k-NN method is effective in improving performance over OctoMap. Through our analysis, the key findings are as follows: • The k-NN model is nonsensitive to different types of distributions. • Parameter is of lower impact than other k-NN parameters. • Through grid search optimisation, the optimal performance of OctoMap can be improved by the k-NN method.
In the future, the computational time can be optimised. The number of combinations of parameters can be potentially reduced before computation. Point clouds generated from other types of sensors will be used to test our method. In addition, the distribution of the average distance in different environments can be investigated.